#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorE,        tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorD,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)

#include "JoystickDriver.c"
#include "Recorder1.c"
#include "Recorder2.c"
task main()
{
	nMotorEncoder[motorA] = 0;
	nMotorEncoder[motorB] = 0;
	int index = 0;
	int x;
	string xString;
	string yString;
	int y;
	int s;
	int LeftArray[50];
	int RightArray[50];
	while(true)														 // Infinite loop:
	{
		getJoystickSettings(joystick);

		if(abs(joystick.joy1_y1) > 10)	 // If the right analog stick's Y-axis readings are either above or below the Q:
		{
			motor[motorD] = (30);					// Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
		}
		else
		{
			motor[motorD] = 0;												// Motor D is stopped with a power level of 0.
		}


		if(abs(joystick.joy1_y2) > 10)	 // If the left analog stick's Y-axis readings are either above or below the Q:
		{
			motor[motorE] = (30);					// Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
		}
		else																		// Else if the readings are within the Q:
		{
			motor[motorE] = 0;												// Motor E is stopped with a power level of 0.
		}
		if(joy1Btn(1))
		{
		  wait1Msec(1000);
			x = nMotorEncoder[motorD];
			y = nMotorEncoder[motorE];
			WriteEncoders(x, y, index);
			xString = "Right: " + x;
			yString = " Left: " + y;
			writeDebugStreamLine(xString);
			writeDebugStreamLine(yString);
			writeDebugStreamLine("");
			nMotorEncoder[motorD] = 0;
			nMotorEncoder[motorE] = 0;
			// RightArray[index] = x;
			//LeftArray[index] = y;
			index++;
		}
	}
}
